#include "ipRoberts.h"

namespace Torch {

real ipRoberts::kx[] = { 0, 0, 0, 0, -1, 0,  0, 0, 1};
real ipRoberts::ky[] = { 0, 0, 0, 0, 0, 1, 0, -1, 0};
//real ipRoberts::kx[] = { 0, 0, 0, -1};
//real ipRoberts::ky[] = { 0, 1,-1, 0};

ipRoberts::ipRoberts (int width_, int height_, const char *coding_) : ipCore(coding_)
{
	width = width_;
	height = height_;
	
   	addROption("threshold", &threshold, 10, "threshold");

	seq_out = new(allocator) Sequence(4, width_*height_*n_planes);
}

void ipRoberts::process(Sequence *seq_in)
{
	real *gradientx = seq_out->frames[0];
	real *gradienty = seq_out->frames[1];
	real *gradientmagnitude = seq_out->frames[2];
	real *edges = seq_out->frames[3];

	real sumx;
	real sumy;
	real sum_gradientmagnitude;
	int ii, jj;
	
	switch(coding_id)
	{
	case 0:
		sum_gradientmagnitude = 0.0;

		for(int i = 0; i < height; i++)
      		{
			int offset = i * width;
		
	        	for(int j = 0; j < width; j++)
			{
	  			sumx = 0.0;
	  			sumy = 0.0;
			
		  		for(int n = 0; n < 3; n++)
		    			for(int m = 0; m < 3; m++)
	    				{
	      					ii = i;
		      				jj = j;
		      				if((i == 0) && (n == 0)) ii += 1;				 
	      					if((j == 0) && (m == 0)) jj += 1;					 
	      					if((i == (height - 1)) && (n == (3 - 1))) ii -= 1;
	      					if((j == (width - 1)) && (m == (3 - 1))) jj -= 1;
	      
		      				sumx += kx[m + n * 3] * seq_in->frames[0][jj + m - 1 + (ii + n - 1) * width];
		      				sumy += ky[m + n * 3] * seq_in->frames[0][jj + m - 1 + (ii + n - 1) * width];
	    				}
			
				int index = j + offset;
			
        	    		gradientx[index] = sumx;
            			gradienty[index] = sumy;

				real gradientmagnitude_ = sqrt(sumx*sumx+sumy*sumy);
	
				if(gradientmagnitude_ < 0.0) gradientmagnitude_ = 0.0;
				else if(gradientmagnitude_ > 255.0) gradientmagnitude_ = 255.0;

				if(threshold == -1)
					sum_gradientmagnitude += gradientmagnitude_;

				gradientmagnitude[index] = gradientmagnitude_;
			
				if(gradientmagnitude_ > threshold) edges[index] = 255.0;
				else edges[index] = 0.0;
		 	}
	      	}
      	
		if(threshold == -1)
		{
		   	sum_gradientmagnitude /= seq_out->frame_size;
		
			for(int i = 0; i < seq_out->frame_size; i++)
		   		if(gradientmagnitude[i] > sum_gradientmagnitude) edges[i] = 255.0;
				else edges[i] = 0.0;
		}
		break;

	case 1:
		message("ipSobel::process() not implemented for RGB format");
		break;
		
	case 2:
		message("ipSobel::process() not implemented for YUV format");
		break;

	case 3:
		sum_gradientmagnitude = 0.0;

		for(int i = 0; i < height; i++)
      		{
			int offset = i * width;
		
	        	for(int j = 0; j < width; j++)
			{
	  			sumx = 0.0;
	  			sumy = 0.0;
			
		  		for(int n = 0; n < 3; n++)
		    			for(int m = 0; m < 3; m++)
	    				{
	      					ii = i;
		      				jj = j;
		      				if((i == 0) && (n == 0)) ii += 1;				 
	      					if((j == 0) && (m == 0)) jj += 1;					 
	      					if((i == (height - 1)) && (n == (3 - 1))) ii -= 1;
	      					if((j == (width - 1)) && (m == (3 - 1))) jj -= 1;
	      
		      				sumx += kx[m + n * 3] * seq_in->frames[0][jj + m - 1 + (ii + n - 1) * width];
		      				sumy += ky[m + n * 3] * seq_in->frames[0][jj + m - 1 + (ii + n - 1) * width];
	    				}
			
				int index = j + offset;
			
        	    		gradientx[index] = sumx;
            			gradienty[index] = sumy;

				real gradientmagnitude_ = sqrt(sumx*sumx+sumy*sumy);
	
				if(gradientmagnitude_ < 0.0) gradientmagnitude_ = 0.0;
				else if(gradientmagnitude_ > 1.0) gradientmagnitude_ = 1.0;

				if(threshold == -1)
					sum_gradientmagnitude += gradientmagnitude_;

				gradientmagnitude[index] = gradientmagnitude_;
			
				if(gradientmagnitude_ > threshold) edges[index] = 1.0;
				else edges[index] = 0.0;
		 	}
	      	}
      	
		if(threshold == -1)
		{
		   	sum_gradientmagnitude /= seq_out->frame_size;
		
			for(int i = 0; i < seq_out->frame_size; i++)
		   		if(gradientmagnitude[i] > sum_gradientmagnitude) edges[i] = 1.0;
				else edges[i] = 0.0;
		}
		break;
	}
}

ipRoberts::~ipRoberts()
{
}

}

